/*
 * controller.h
 *
 *  Created on: Nov 26, 2012
 *      Author: michiel
 */

#ifndef CONTROLLER_H_
#define CONTROLLER_H_

#include <ros/ros.h>
#include <sensor_msgs/Joy.h>

#include "topic/topic.h"
#include "PS3_buttons.h"
#include "comm/eyes_msg.h"
#include "comm/ears_msg.h"
#include "comm/eyebrows_msg.h"
#include "comm/trunk_msg.h"
#include "comm/emotion_msg.h"

class Controller {
private:
	bool _pressed;
protected:
	ros::NodeHandle nh_;
	ros::Subscriber joy_sub_;
	ros::Publisher eyes_pub_;
	ros::Publisher ears_pub_;
	ros::Publisher eyebrows_pub_;
	ros::Publisher trunk_pub_;
	ros::Publisher emo_pub_;

	int eyebrowstate_;
	int trunkstate_;
	int earstate_;

	sensor_msgs::Joy::_buttons_type buttons_;
	sensor_msgs::Joy::_axes_type axes_;

	float leftStickPositionXAxis_;
	float leftStickPositionYAxis_;
	float rightStickPositionXAxis_;
	float rightStickPositionYAxis_;

	float leftTrigger_;
	float rightTrigger_;

	int buttonUp_;
	int buttonDown_;
	int buttonTriangle_;
	int buttonCross_;
	int buttonSquare_;
	int buttonCircle_;

	bool upPressed_;
	bool downPressed_;
	bool trianglePressed_;
	bool crossPressed_;
	bool circlePressed_;
	bool squarePressed_;
public:
	Controller() {
		_pressed = false;

		leftStickPositionXAxis_		= 0.0;
		leftStickPositionYAxis_		= 0.0;
		rightStickPositionXAxis_	= 0.0;
		rightStickPositionYAxis_	= 0.0;

		leftTrigger_	= 0.0;
		rightTrigger_	= 0.0;

		buttonUp_				= 0;
		buttonDown_			= 0;
		buttonTriangle_	= 0;
		buttonCross_		= 0;
		buttonCircle_		= 0;
		buttonSquare_		= 0;

		upPressed_				= false;
		downPressed_			= false;
		crossPressed_			= false;
		trianglePressed_	= false;
		circlePressed_		= false;
		squarePressed_		=	false;

		eyebrowstate_ = EYEBROWS_NEUTRAL;
		trunkstate_		= TRUNK_NEUTRAL;
		earstate_			= EARS_NEUTRAL;

		joy_sub_			= nh_.subscribe(Topic::PS3_JOY, 1, &Controller::controllerCallback, this);
	}
	~Controller() {
		nh_.shutdown();
	}
	bool isPressed() { return _pressed; }
	void setPressed(bool state) { _pressed = state; }
	void init();
	void spin();
	void controllerCallback(const sensor_msgs::Joy::ConstPtr &message);
	void browStateNext();
	void browStatePrev();
	void trunkStateNext();
	void trunkStatePrev();
};

#endif /* CONTROLLER_H_ */
